#include "State_Fall.h"
#include "Dog.h"

#define KP_F 3.0f
#define KD_F 1.6f

#define KP_B 3.0f
#define KD_B 1.6f

void Fall_Enter(void)
{
    UART_Print("Fall enter\n");
}

void Fall_Run(void)
{
    HT_SetPosition(dog.legs[0].motorF, 0, KP_F, KD_F, 0);
    HT_SetPosition(dog.legs[0].motorB, 0, KP_B, KD_B, 0);
    HT_SetPosition(dog.legs[1].motorF, 0, KP_F, KD_F, 0);
    HT_SetPosition(dog.legs[1].motorB, 0, KP_B, KD_B, 0);
    HT_SetPosition(dog.legs[2].motorF, 0, KP_F, KD_F, 0);
    HT_SetPosition(dog.legs[2].motorB, 0, KP_B, KD_B, 0);
    HT_SetPosition(dog.legs[3].motorF, 0, KP_F, KD_F, 0);
    HT_SetPosition(dog.legs[3].motorB, 0, KP_B, KD_B, 0);
}
void Fall_Exit(void)
{
    UART_Print("Fall exit\n");
}